#include "Contact.h"

void Contact::resolve(float frameTime)
{
	resolveVelocity(frameTime);
	resolveInterpenetration(frameTime);
}

float Contact::calculateSeparatingVelocity() const
{
	D3DXVECTOR3 relativeVelocity = particle[0]->velocity;
	if (particle[1]) relativeVelocity -= particle[1]->velocity;
	return D3DXVec3Dot(&relativeVelocity, &contactNormal);
}

/*
void Contact::resolveVelocity(float frameTime)
{
	// Find the velocity in the direction of the contact.
	float separatingVelocity = calculateSeparatingVelocity();

	// Check if it needs to be resolved.
	if (separatingVelocity > 0)
	{
		// The contact is either separating, or stationary;
		// no impulse is required.
		return;
	}

	// Calculate the new separating velocity.
	float newSepVelocity = -separatingVelocity * restitution;

	float deltaVelocity = newSepVelocity - separatingVelocity;

	// W apply the change in velocity to each object in proportion to
	// thier inverse mass (i.e., those with lower inverse mass [higher
	// actual mass] get less change in velocity).
	float totalInverseMass = particle[0]->inverseMass;
	if (particle[1]) totalInverseMass += particle[1]->inverseMass;

	// If all particles have infinite mass, then impulses have no effect.
	if (totalInverseMass <= 0) return;

	// Calcluate the impulse to apply.
	float impulse = deltaVelocity / totalInverseMass;

	// Find the amount of impulse per unit of inverse mass.
	D3DXVECTOR3 impulsePerIMass = contactNormal * impulse;

	// Apply impulses: they are applied in the direction of the contact.
	// and are proportional to the inverse mass.
	particle[0]->velocity = particle[0]->velocity + 
		impulsePerIMass * particle[0]->inverseMass;
	if (particle[1])
	{
		// Particle 1 goes in the opposite direction
		particle[1]->velocity = particle[1]->velocity + 
			impulsePerIMass * -particle[1]->inverseMass;
	}
}
*/

void Contact::resolveVelocity(float frameTime)
{
	// Find the velocity in the direction of the contact.
	float separatingVelocity = calculateSeparatingVelocity();

	// Check if it needs to be resolved.
	if (separatingVelocity > 0)
	{
		// The contact is either separating, or stationary; there's
		// no impulse required.
		return;
	}

	// Calculate the new separating velocity.
	float newSepVelocity = - separatingVelocity * restitution;

	// Check the velocity buildup due to acceleration only.
	D3DXVECTOR3 accCausedVelocity = particle[0]->acceleration;
	if (particle[1]) accCausedVelocity -= particle[1]->acceleration;
	float accCausedSepVelocity = 
		D3DXVec3Dot(&accCausedVelocity, &contactNormal) * frameTime;

	// If we've for a closing velocity due to acceleration buildup,
	// remove it from the new separating velocity.
	if (accCausedSepVelocity < 0)
	{
		newSepVelocity += restitution * accCausedSepVelocity;

		// Make sure we haven't removed more than was
		// there to remove.
		if (newSepVelocity < 0) newSepVelocity = 0;
	}

	float deltaVelocity = newSepVelocity - separatingVelocity;

	// We apply the change in velocity to each object in proportion
	// to their inverse mass (i.e., those with lower inverse mass [higher]
	// actual mass] get less change in velocity).
	float totalInverseMass = particle[0]->inverseMass;
	if (particle[1]) totalInverseMass += particle[1]->inverseMass;

	// If all particles have infinite mass, then impulses have no effect.
	if (totalInverseMass <= 0) return;

	// Calculate the impulse per unit of inverse mass.
	float impulse = deltaVelocity / totalInverseMass;

	// Find the amount of impulse per unit of inverse mass.
	D3DXVECTOR3 impulsePerIMass = contactNormal * impulse;

	// Apply impulses: they are applied in the direction of the contact.,
	// and are proportional to the inverse mass.
	particle[0]->velocity += impulsePerIMass * particle[0]->inverseMass;
	if (particle[1])
	{
		// Particle 1 goes in the opposite direction.
		particle[1]->velocity += impulsePerIMass * 
			-particle[1]->inverseMass;
	}
}

void Contact::resolveInterpenetration(float frameTime)
{
	// If we don't have any penetration, skip this step.
	if (penetration <= 0) return;

	// The movement of each object is based on their inverse mass,
	// so total that.
	float totalInverseMass = particle[0]->inverseMass;
	if (particle[1]) totalInverseMass += particle[1]->inverseMass;

	// If all particles have infinite mass, then we do nothing.
	if (totalInverseMass <= 0) return;

	// Find the amount of penetration resolution per unit
	// of inverse mass.
	D3DXVECTOR3 movePerIMass = 
		contactNormal * (penetration / totalInverseMass);

	// Calculate the movement amounts.
	particleMovement[0] = movePerIMass * particle[0]->inverseMass;
	if (particle[1]) {
		particleMovement[1] = movePerIMass * -particle[1]->inverseMass;
	} else {
		particleMovement[1] = D3DXVECTOR3(0, 0, 0);
	}

	// Apply the penetration resolution.
	particle[0]->position += particleMovement[0];

	if (particle[1])
		particle[1]->position += particleMovement[1];
}